/*
 * Copyright 1997-2008 Sun Microsystems, Inc.  All Rights Reserved.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * This code is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License version 2 only, as
 * published by the Free Software Foundation.  Sun designates this
 * particular file as subject to the "Classpath" exception as provided
 * by Sun in the LICENSE file that accompanied this code.
 *
 * This code is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * version 2 for more details (a copy is included in the LICENSE file that
 * accompanied this code).
 *
 * You should have received a copy of the GNU General Public License version
 * 2 along with this work; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
 * CA 95054 USA or visit www.sun.com if you need additional information or
 * have any questions.
 *
 */

package forward.Libraries.javax.vecmath;

/**
 * A 4 element unit quaternion represented by single precision floating point
 * x,y,z,w coordinates. The quaternion is always normalized.
 *
 */
public class Quat4f extends Tuple4f implements java.io.Serializable {

	// Combatible with 1.1
	static final long serialVersionUID = 2675933778405442383L;

	final static double EPS = 0.000001;
	final static double EPS2 = 1.0e-30;
	final static double PIO2 = 1.57079632679;

	/**
	 * Constructs and initializes a Quat4f from the specified xyzw coordinates.
	 * 
	 * @param x the x coordinate
	 * @param y the y coordinate
	 * @param z the z coordinate
	 * @param w the w scalar component
	 */
	public Quat4f(float x, float y, float z, float w) {
		float mag;
		mag = (float) (1.0 / Math.sqrt(x * x + y * y + z * z + w * w));
		this.x = x * mag;
		this.y = y * mag;
		this.z = z * mag;
		this.w = w * mag;

	}

	/**
	 * Constructs and initializes a Quat4f from the array of length 4.
	 * 
	 * @param q the array of length 4 containing xyzw in order
	 */
	public Quat4f(float[] q) {
		float mag;
		mag = (float) (1.0 / Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]));
		x = q[0] * mag;
		y = q[1] * mag;
		z = q[2] * mag;
		w = q[3] * mag;

	}

	/**
	 * Constructs and initializes a Quat4f from the specified Quat4f.
	 * 
	 * @param q1 the Quat4f containing the initialization x y z w data
	 */
	public Quat4f(Quat4f q1) {
		super(q1);
	}

	/**
	 * Constructs and initializes a Quat4f from the specified Quat4d.
	 * 
	 * @param q1 the Quat4d containing the initialization x y z w data
	 */
	public Quat4f(Quat4d q1) {
		super(q1);
	}

	/**
	 * Constructs and initializes a Quat4f from the specified Tuple4f.
	 * 
	 * @param t1 the Tuple4f containing the initialization x y z w data
	 */
	public Quat4f(Tuple4f t1) {
		float mag;
		mag = (float) (1.0 / Math.sqrt(t1.x * t1.x + t1.y * t1.y + t1.z * t1.z + t1.w * t1.w));
		x = t1.x * mag;
		y = t1.y * mag;
		z = t1.z * mag;
		w = t1.w * mag;

	}

	/**
	 * Constructs and initializes a Quat4f from the specified Tuple4d.
	 * 
	 * @param t1 the Tuple4d containing the initialization x y z w data
	 */
	public Quat4f(Tuple4d t1) {
		double mag;
		mag = 1.0 / Math.sqrt(t1.x * t1.x + t1.y * t1.y + t1.z * t1.z + t1.w * t1.w);
		x = (float) (t1.x * mag);
		y = (float) (t1.y * mag);
		z = (float) (t1.z * mag);
		w = (float) (t1.w * mag);
	}

	/**
	 * Constructs and initializes a Quat4f to (0.0,0.0,0.0,0.0).
	 */
	public Quat4f() {
		super();
	}

	/**
	 * Sets the value of this quaternion to the conjugate of quaternion q1.
	 * 
	 * @param q1 the source vector
	 */
	public final void conjugate(Quat4f q1) {
		this.x = -q1.x;
		this.y = -q1.y;
		this.z = -q1.z;
		this.w = q1.w;
	}

	/**
	 * Sets the value of this quaternion to the conjugate of itself.
	 */
	public final void conjugate() {
		this.x = -this.x;
		this.y = -this.y;
		this.z = -this.z;
	}

	/**
	 * Sets the value of this quaternion to the quaternion product of quaternions q1
	 * and q2 (this = q1 * q2). Note that this is safe for aliasing (e.g. this can
	 * be q1 or q2).
	 * 
	 * @param q1 the first quaternion
	 * @param q2 the second quaternion
	 */
	public final void mul(Quat4f q1, Quat4f q2) {
		if (this != q1 && this != q2) {
			this.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
			this.x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
			this.y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
			this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
		} else {
			float x, y, w;

			w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
			x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
			y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
			this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
			this.w = w;
			this.x = x;
			this.y = y;
		}
	}

	/**
	 * Sets the value of this quaternion to the quaternion product of itself and q1
	 * (this = this * q1).
	 * 
	 * @param q1 the other quaternion
	 */
	public final void mul(Quat4f q1) {
		float x, y, w;

		w = this.w * q1.w - this.x * q1.x - this.y * q1.y - this.z * q1.z;
		x = this.w * q1.x + q1.w * this.x + this.y * q1.z - this.z * q1.y;
		y = this.w * q1.y + q1.w * this.y - this.x * q1.z + this.z * q1.x;
		this.z = this.w * q1.z + q1.w * this.z + this.x * q1.y - this.y * q1.x;
		this.w = w;
		this.x = x;
		this.y = y;
	}

	/**
	 * Multiplies quaternion q1 by the inverse of quaternion q2 and places the value
	 * into this quaternion. The value of both argument quaternions is preservered
	 * (this = q1 * q2^-1).
	 * 
	 * @param q1 the first quaternion
	 * @param q2 the second quaternion
	 */
	public final void mulInverse(Quat4f q1, Quat4f q2) {
		Quat4f tempQuat = new Quat4f(q2);

		tempQuat.inverse();
		this.mul(q1, tempQuat);
	}

	/**
	 * Multiplies this quaternion by the inverse of quaternion q1 and places the
	 * value into this quaternion. The value of the argument quaternion is preserved
	 * (this = this * q^-1).
	 * 
	 * @param q1 the other quaternion
	 */
	public final void mulInverse(Quat4f q1) {
		Quat4f tempQuat = new Quat4f(q1);

		tempQuat.inverse();
		this.mul(tempQuat);
	}

	/**
	 * Sets the value of this quaternion to quaternion inverse of quaternion q1.
	 * 
	 * @param q1 the quaternion to be inverted
	 */
	public final void inverse(Quat4f q1) {
		float norm;

		norm = 1.0f / (q1.w * q1.w + q1.x * q1.x + q1.y * q1.y + q1.z * q1.z);
		this.w = norm * q1.w;
		this.x = -norm * q1.x;
		this.y = -norm * q1.y;
		this.z = -norm * q1.z;
	}

	/**
	 * Sets the value of this quaternion to the quaternion inverse of itself.
	 */
	public final void inverse() {
		float norm;

		norm = 1.0f / (this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z);
		this.w *= norm;
		this.x *= -norm;
		this.y *= -norm;
		this.z *= -norm;
	}

	/**
	 * Sets the value of this quaternion to the normalized value of quaternion q1.
	 * 
	 * @param q1 the quaternion to be normalized.
	 */
	public final void normalize(Quat4f q1) {
		float norm;

		norm = (q1.x * q1.x + q1.y * q1.y + q1.z * q1.z + q1.w * q1.w);

		if (norm > 0.0f) {
			norm = 1.0f / (float) Math.sqrt(norm);
			this.x = norm * q1.x;
			this.y = norm * q1.y;
			this.z = norm * q1.z;
			this.w = norm * q1.w;
		} else {
			this.x = (float) 0.0;
			this.y = (float) 0.0;
			this.z = (float) 0.0;
			this.w = (float) 0.0;
		}
	}

	/**
	 * Normalizes the value of this quaternion in place.
	 */
	public final void normalize() {
		float norm;

		norm = (this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);

		if (norm > 0.0f) {
			norm = 1.0f / (float) Math.sqrt(norm);
			this.x *= norm;
			this.y *= norm;
			this.z *= norm;
			this.w *= norm;
		} else {
			this.x = (float) 0.0;
			this.y = (float) 0.0;
			this.z = (float) 0.0;
			this.w = (float) 0.0;
		}
	}

	/**
	 * Sets the value of this quaternion to the rotational component of the passed
	 * matrix.
	 * 
	 * @param m1 the Matrix4f
	 */
	public final void set(Matrix4f m1) {
		float ww = 0.25f * (m1.m00 + m1.m11 + m1.m22 + m1.m33);

		if (ww >= 0) {
			if (ww >= EPS2) {
				this.w = (float) Math.sqrt((double) ww);
				ww = 0.25f / this.w;
				this.x = (m1.m21 - m1.m12) * ww;
				this.y = (m1.m02 - m1.m20) * ww;
				this.z = (m1.m10 - m1.m01) * ww;
				return;
			}
		} else {
			this.w = 0;
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.w = 0;
		ww = -0.5f * (m1.m11 + m1.m22);

		if (ww >= 0) {
			if (ww >= EPS2) {
				this.x = (float) Math.sqrt((double) ww);
				ww = 1.0f / (2.0f * this.x);
				this.y = m1.m10 * ww;
				this.z = m1.m20 * ww;
				return;
			}
		} else {
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.x = 0;
		ww = 0.5f * (1.0f - m1.m22);

		if (ww >= EPS2) {
			this.y = (float) Math.sqrt((double) ww);
			this.z = m1.m21 / (2.0f * this.y);
			return;
		}

		this.y = 0;
		this.z = 1;
	}

	/**
	 * Sets the value of this quaternion to the rotational component of the passed
	 * matrix.
	 * 
	 * @param m1 the Matrix4d
	 */
	public final void set(Matrix4d m1) {
		double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + m1.m33);

		if (ww >= 0) {
			if (ww >= EPS2) {
				this.w = (float) Math.sqrt(ww);
				ww = 0.25 / this.w;
				this.x = (float) ((m1.m21 - m1.m12) * ww);
				this.y = (float) ((m1.m02 - m1.m20) * ww);
				this.z = (float) ((m1.m10 - m1.m01) * ww);
				return;
			}
		} else {
			this.w = 0;
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.w = 0;
		ww = -0.5 * (m1.m11 + m1.m22);
		if (ww >= 0) {
			if (ww >= EPS2) {
				this.x = (float) Math.sqrt(ww);
				ww = 0.5 / this.x;
				this.y = (float) (m1.m10 * ww);
				this.z = (float) (m1.m20 * ww);
				return;
			}
		} else {
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.x = 0;
		ww = 0.5 * (1.0 - m1.m22);
		if (ww >= EPS2) {
			this.y = (float) Math.sqrt(ww);
			this.z = (float) (m1.m21 / (2.0 * (double) (this.y)));
			return;
		}

		this.y = 0;
		this.z = 1;
	}

	/**
	 * Sets the value of this quaternion to the rotational component of the passed
	 * matrix.
	 * 
	 * @param m1 the Matrix3f
	 */
	public final void set(Matrix3f m1) {
		float ww = 0.25f * (m1.m00 + m1.m11 + m1.m22 + 1.0f);

		if (ww >= 0) {
			if (ww >= EPS2) {
				this.w = (float) Math.sqrt((double) ww);
				ww = 0.25f / this.w;
				this.x = (m1.m21 - m1.m12) * ww;
				this.y = (m1.m02 - m1.m20) * ww;
				this.z = (m1.m10 - m1.m01) * ww;
				return;
			}
		} else {
			this.w = 0;
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.w = 0;
		ww = -0.5f * (m1.m11 + m1.m22);
		if (ww >= 0) {
			if (ww >= EPS2) {
				this.x = (float) Math.sqrt((double) ww);
				ww = 0.5f / this.x;
				this.y = m1.m10 * ww;
				this.z = m1.m20 * ww;
				return;
			}
		} else {
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.x = 0;
		ww = 0.5f * (1.0f - m1.m22);
		if (ww >= EPS2) {
			this.y = (float) Math.sqrt((double) ww);
			this.z = m1.m21 / (2.0f * this.y);
			return;
		}

		this.y = 0;
		this.z = 1;
	}

	/**
	 * Sets the value of this quaternion to the rotational component of the passed
	 * matrix.
	 * 
	 * @param m1 the Matrix3d
	 */
	public final void set(Matrix3d m1) {
		double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + 1.0f);

		if (ww >= 0) {
			if (ww >= EPS2) {
				this.w = (float) Math.sqrt(ww);
				ww = 0.25 / this.w;
				this.x = (float) ((m1.m21 - m1.m12) * ww);
				this.y = (float) ((m1.m02 - m1.m20) * ww);
				this.z = (float) ((m1.m10 - m1.m01) * ww);
				return;
			}
		} else {
			this.w = 0;
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.w = 0;
		ww = -0.5 * (m1.m11 + m1.m22);
		if (ww >= 0) {
			if (ww >= EPS2) {
				this.x = (float) Math.sqrt(ww);
				ww = 0.5 / this.x;
				this.y = (float) (m1.m10 * ww);
				this.z = (float) (m1.m20 * ww);
				return;
			}
		} else {
			this.x = 0;
			this.y = 0;
			this.z = 1;
			return;
		}

		this.x = 0;
		ww = 0.5 * (1.0 - m1.m22);
		if (ww >= EPS2) {
			this.y = (float) Math.sqrt(ww);
			this.z = (float) (m1.m21 / (2.0 * (double) (this.y)));
			return;
		}

		this.y = 0;
		this.z = 1;
	}

	/**
	 * Sets the value of this quaternion to the equivalent rotation of the AxisAngle
	 * argument.
	 * 
	 * @param a the AxisAngle to be emulated
	 */
	public final void set(AxisAngle4f a) {
		float mag, amag;
		// Quat = cos(theta/2) + sin(theta/2)(roation_axis)
		amag = (float) Math.sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
		if (amag < EPS) {
			w = 0.0f;
			x = 0.0f;
			y = 0.0f;
			z = 0.0f;
		} else {
			amag = 1.0f / amag;
			mag = (float) Math.sin(a.angle / 2.0);
			w = (float) Math.cos(a.angle / 2.0);
			x = a.x * amag * mag;
			y = a.y * amag * mag;
			z = a.z * amag * mag;
		}
	}

	/**
	 * Sets the value of this quaternion to the equivalent rotation of the AxisAngle
	 * argument.
	 * 
	 * @param a the AxisAngle to be emulated
	 */
	public final void set(AxisAngle4d a) {
		float mag, amag;
		// Quat = cos(theta/2) + sin(theta/2)(roation_axis)

		amag = (float) (1.0 / Math.sqrt(a.x * a.x + a.y * a.y + a.z * a.z));

		if (amag < EPS) {
			w = 0.0f;
			x = 0.0f;
			y = 0.0f;
			z = 0.0f;
		} else {
			amag = 1.0f / amag;
			mag = (float) Math.sin(a.angle / 2.0);
			w = (float) Math.cos(a.angle / 2.0);
			x = (float) a.x * amag * mag;
			y = (float) a.y * amag * mag;
			z = (float) a.z * amag * mag;
		}

	}

	/**
	 * Performs a great circle interpolation between this quaternion and the
	 * quaternion parameter and places the result into this quaternion.
	 * 
	 * @param q1    the other quaternion
	 * @param alpha the alpha interpolation parameter
	 */
	public final void interpolate(Quat4f q1, float alpha) {
		// From "Advanced Animation and Rendering Techniques"
		// by Watt and Watt pg. 364, function as implemented appeared to be
		// incorrect. Fails to choose the same quaternion for the double
		// covering. Resulting in change of direction for rotations.
		// Fixed function to negate the first quaternion in the case that the
		// dot product of q1 and this is negative. Second case was not needed.

		double dot, s1, s2, om, sinom;

		dot = x * q1.x + y * q1.y + z * q1.z + w * q1.w;

		if (dot < 0) {
			// negate quaternion
			q1.x = -q1.x;
			q1.y = -q1.y;
			q1.z = -q1.z;
			q1.w = -q1.w;
			dot = -dot;
		}

		if ((1.0 - dot) > EPS) {
			om = Math.acos(dot);
			sinom = Math.sin(om);
			s1 = Math.sin((1.0 - alpha) * om) / sinom;
			s2 = Math.sin(alpha * om) / sinom;
		} else {
			s1 = 1.0 - alpha;
			s2 = alpha;
		}

		w = (float) (s1 * w + s2 * q1.w);
		x = (float) (s1 * x + s2 * q1.x);
		y = (float) (s1 * y + s2 * q1.y);
		z = (float) (s1 * z + s2 * q1.z);
	}

	/**
	 * Performs a great circle interpolation between quaternion q1 and quaternion q2
	 * and places the result into this quaternion.
	 * 
	 * @param q1    the first quaternion
	 * @param q2    the second quaternion
	 * @param alpha the alpha interpolation parameter
	 */
	public final void interpolate(Quat4f q1, Quat4f q2, float alpha) {
		// From "Advanced Animation and Rendering Techniques"
		// by Watt and Watt pg. 364, function as implemented appeared to be
		// incorrect. Fails to choose the same quaternion for the double
		// covering. Resulting in change of direction for rotations.
		// Fixed function to negate the first quaternion in the case that the
		// dot product of q1 and this is negative. Second case was not needed.

		double dot, s1, s2, om, sinom;

		dot = q2.x * q1.x + q2.y * q1.y + q2.z * q1.z + q2.w * q1.w;

		if (dot < 0) {
			// negate quaternion
			q1.x = -q1.x;
			q1.y = -q1.y;
			q1.z = -q1.z;
			q1.w = -q1.w;
			dot = -dot;
		}

		if ((1.0 - dot) > EPS) {
			om = Math.acos(dot);
			sinom = Math.sin(om);
			s1 = Math.sin((1.0 - alpha) * om) / sinom;
			s2 = Math.sin(alpha * om) / sinom;
		} else {
			s1 = 1.0 - alpha;
			s2 = alpha;
		}
		w = (float) (s1 * q1.w + s2 * q2.w);
		x = (float) (s1 * q1.x + s2 * q2.x);
		y = (float) (s1 * q1.y + s2 * q2.y);
		z = (float) (s1 * q1.z + s2 * q2.z);
	}

}
